package star.crust.cartspace.framework;

import java.util.ArrayList;

public class CrtObb extends CrtBoxing {
	private float[][] pR;
	private float[] pT;

	public CrtObb(float[][] pR, float[] pT, ArrayList<CrtMeshPrimitive> meshsPrimitive) {
		super(meshsPrimitive,"OBB");
		this.vector = new float[3];
		this.pR = pR;
		this.pT = pT;
	}

	/**
	 * Il metodo imposta la matrice di rotazione per 
	 * il volume OBB
	 * @param pR
	 */
	public void setpR(float[][] pR) {
		this.pR = pR;
	}

	public float[][] getpR() {
		return this.pR;
	}

	/**
	 * Il metodo imposta il vettore di traslazione
	 * per l'oggetto OBB
	 * @param pT
	 */
	public void setpT(float[] pT) {
		this.pT = pT;
	}

	public float[] getpT() {
		return this.pT;
	}

	public float size() {
		return super.vector[0];
	}

	/**
	 * I metodi verificano la presenza di collisioni
	 * tra le singole mesh
	 * @param b2
	 * @param RAPID_ms
	 * @param RAPID_mR
	 * @param RAPID_mT
	 * @return
	 */
	public boolean	tri_contact(CrtObb b2, float RAPID_ms, float[][] RAPID_mR, float[] RAPID_mT) {

		float[] i1  = new float[3];
		float[] i2  = new float[3];
		float[] i3  = new float[3];

		float[] box1_p1  = CrtObbMath.FloatXYZ2Vett(this.meshsPrimitive.get(0).getVertex(0).position()); 
		float[] box1_p2  = CrtObbMath.FloatXYZ2Vett(this.meshsPrimitive.get(0).getVertex(1).position()); 
		float[] box1_p3  = CrtObbMath.FloatXYZ2Vett(this.meshsPrimitive.get(0).getVertex(2).position()); 
		float[] box2_p1  = CrtObbMath.FloatXYZ2Vett(b2.meshsPrimitive.get(0).getVertex(0).position()); 
		float[] box2_p2  = CrtObbMath.FloatXYZ2Vett(b2.meshsPrimitive.get(0).getVertex(1).position()); 
		float[] box2_p3  = CrtObbMath.FloatXYZ2Vett(b2.meshsPrimitive.get(0).getVertex(2).position()); 

		i1 = CrtObbMath.sMxVpV(RAPID_ms, RAPID_mR, box1_p1, RAPID_mT);
		i2 = CrtObbMath.sMxVpV(RAPID_ms, RAPID_mR, box1_p2, RAPID_mT);
		i3 = CrtObbMath.sMxVpV(RAPID_ms, RAPID_mR, box1_p3, RAPID_mT);

		boolean f = tri_contact(i1, i2, i3, box2_p1,box2_p2, box2_p3);

		return CrtObbMath.RAPID_OK;
	}

	private boolean tri_contact (float[] P1, float[] P2, float[] P3, float[] Q1, float[] Q2, float[] Q3) {
		float[] p1  = new float[3], p2  = new float[3], p3  = new float[3];
		float[] q1  = new float[3], q2  = new float[3], q3  = new float[3];
		float[] e1  = new float[3], e2  = new float[3], e3  = new float[3];
		float[] f1  = new float[3], f2  = new float[3], f3  = new float[3];
		float[] g1  = new float[3], g2  = new float[3], g3  = new float[3];
		float[] h1  = new float[3], h2  = new float[3], h3  = new float[3];

		float[] n1  = new float[3], m1  = new float[3];
		float[] z  = new float[3];

		float[] ef11  = new float[3], ef12  = new float[3], ef13  = new float[3];
		float[] ef21  = new float[3], ef22  = new float[3], ef23  = new float[3];
		float[] ef31  = new float[3], ef32  = new float[3], ef33  = new float[3];

		z[0] = 0;  z[1] = 0;  z[2] = 0;

		p1[0] = P1[0] - P1[0];  p1[1] = P1[1] - P1[1];  p1[2] = P1[2] - P1[2];
		p2[0] = P2[0] - P1[0];  p2[1] = P2[1] - P1[1];  p2[2] = P2[2] - P1[2];
		p3[0] = P3[0] - P1[0];  p3[1] = P3[1] - P1[1];  p3[2] = P3[2] - P1[2];

		q1[0] = Q1[0] - P1[0];  q1[1] = Q1[1] - P1[1];  q1[2] = Q1[2] - P1[2];
		q2[0] = Q2[0] - P1[0];  q2[1] = Q2[1] - P1[1];  q2[2] = Q2[2] - P1[2];
		q3[0] = Q3[0] - P1[0];  q3[1] = Q3[1] - P1[1];  q3[2] = Q3[2] - P1[2];

		e1[0] = p2[0] - p1[0];  e1[1] = p2[1] - p1[1];  e1[2] = p2[2] - p1[2];
		e2[0] = p3[0] - p2[0];  e2[1] = p3[1] - p2[1];  e2[2] = p3[2] - p2[2];
		e3[0] = p1[0] - p3[0];  e3[1] = p1[1] - p3[1];  e3[2] = p1[2] - p3[2];

		f1[0] = q2[0] - q1[0];  f1[1] = q2[1] - q1[1];  f1[2] = q2[2] - q1[2];
		f2[0] = q3[0] - q2[0];  f2[1] = q3[1] - q2[1];  f2[2] = q3[2] - q2[2];
		f3[0] = q1[0] - q3[0];  f3[1] = q1[1] - q3[1];  f3[2] = q1[2] - q3[2];

		n1 = CrtObbMath.VcrossV(e1, e2);
		m1 = CrtObbMath.VcrossV(f1, f2);

		g1 = CrtObbMath.VcrossV(e1, n1);
		g2 = CrtObbMath.VcrossV(e2, n1);
		g3 = CrtObbMath.VcrossV(e3, n1);
		h1 = CrtObbMath.VcrossV(f1, m1);
		h2 = CrtObbMath.VcrossV(f2, m1);
		h3 = CrtObbMath.VcrossV(f3, m1);

		ef11 = CrtObbMath.VcrossV(e1, f1);
		ef12 = CrtObbMath.VcrossV(e1, f2);
		ef13 = CrtObbMath.VcrossV(e1, f3);
		ef21 = CrtObbMath.VcrossV(e2, f1);
		ef22 = CrtObbMath.VcrossV(e2, f2);
		ef23 = CrtObbMath.VcrossV(e2, f3);
		ef31 = CrtObbMath.VcrossV(e3, f1);
		ef32 = CrtObbMath.VcrossV(e3, f2);
		ef33 = CrtObbMath.VcrossV(e3, f3);

		if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false;

		if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false;

		if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false;
		if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false;

		return true;
	}


	/**
	 * Il metodo effettua la proiezione
	 * del volume sull'asse x.
	 * @param ax
	 * @param p1
	 * @param p2
	 * @param p3
	 * @param q1
	 * @param q2
	 * @param q3
	 * @return
	 */
	private boolean project6(float[] ax, float[] p1, float[] p2, float[] p3, float[] q1, float[] q2, float[] q3)	{
		float P1 = CrtObbMath.VdotV(ax, p1);
		float P2 = CrtObbMath.VdotV(ax, p2);
		float P3 = CrtObbMath.VdotV(ax, p3);
		float Q1 = CrtObbMath.VdotV(ax, q1);
		float Q2 = CrtObbMath.VdotV(ax, q2);
		float Q3 = CrtObbMath.VdotV(ax, q3);

		float mx1 = CrtObbMath.max(P1, P2, P3);
		float mn1 = CrtObbMath.min(P1, P2, P3);
		float mx2 = CrtObbMath.max(Q1, Q2, Q3);
		float mn2 = CrtObbMath.min(Q1, Q2, Q3);

		if (mn1 > mx2) return false;
		if (mn2 > mx1) return false;
		return true;
	}
	
	public String toString() {
		String ret = "";
		/*
		ret +="pR:\n"+this.pR[0][0]+" "+this.pR[0][1]+" "+this.pR[0][2]+"\n";
		ret +=this.pR[1][0]+" "+this.pR[1][1]+" "+this.pR[1][2];
		ret +="\n"+this.pR[2][0]+" "+this.pR[2][1]+" "+this.pR[2][2];
		ret +="\npT:"+this.pT[0]+" "+this.pT[1]+" "+this.pT[2];
		ret += "\n"+super.toString()+"\n";
		*/
		ret +=String.format("%1$-" + 10 + "s", this.pR[0][0])+"\t"+String.format("%1$-" + 10 + "s", this.pR[0][1])+"\t"+String.format("%1$-" + 10 + "s", this.pR[0][2])+"\t";
		ret +=String.format("%1$-" + 10 + "s", this.pR[1][0])+"\t"+String.format("%1$-" + 10 + "s", this.pR[1][1])+"\t"+String.format("%1$-" + 10 + "s", this.pR[1][2])+"\t";
		ret +=String.format("%1$-" + 10 + "s", this.pR[2][0])+"\t"+String.format("%1$-" + 10 + "s", this.pR[2][1])+"\t"+String.format("%1$-" + 10 + "s", this.pR[2][2])+"\t";
		ret +=String.format("%1$-" + 10 + "s", this.pT[0])+"\t"+String.format("%1$-" + 10 + "s", this.pT[1])+"\t"+String.format("%1$-" + 10 + "s", this.pT[2])+"\t";
		ret +=super.toString()+"\n";
		return ret;
	}

	public double dblsize() {
		return 0;
	}
}
